

import rospy
import matplotlib.pyplot as plt

from em_planner.msg import SpeedPlannerInfo

def stMapCallback(data):
    obs_s_vec = data.obs_s_vec
    obs_t_vec = data.obs_t_vec

    s_vec = data.s_vec
    t_vec = data.t_vec
    v_vec = data.v_vec

    plt.cla()
    plt.plot(obs_t_vec, obs_s_vec, 'r')
    plt.plot(t_vec, s_vec, 'g')
    plt.grid(True)
    plt.xlim(0, 16)
    plt.ylim(0, 60)
    plt.show(block=False)
    plt.pause(0.05)
    # plt.show()

if __name__ == '__main__':
    rospy.init_node('plot_st', disable_signals=True)

    rospy.Subscriber('speed_planner_info', SpeedPlannerInfo, stMapCallback)

    rospy.spin()